#ifndef _AQ_PID_H_
#define _AQ_PID_H_
#define RATE_XAXIS_PID_IDX          0
#define RATE_YAXIS_PID_IDX          1
#define ZAXIS_PID_IDX               2
#define ATTITUDE_XAXIS_PID_IDX      3
#define ATTITUDE_YAXIS_PID_IDX      4
#define HEADING_HOLD_PID_IDX        5
#define ATTITUDE_GYRO_XAXIS_PID_IDX 6
#define ATTITUDE_GYRO_YAXIS_PID_IDX 7
#define ALTITUDE_HOLD_PID_IDX       8
#define ZDAMPENING_PID_IDX          9
// PID Variables
struct PIDdata {
    float P, I, D;
    float lastPosition;
    // AKA experiments with PID
    float previousPIDTime;
    float integratedError;
    float windupGuard; // Thinking about having individual wind up guards for each PID
} PID[10];
// This struct above declares the variable PID[] to hold each of the PID values for various functions
// The following constants are declared in AeroQuad.h
// ROLL = 0, PITCH = 1, YAW = 2 (used for Arcobatic Mode, gyros only)
// ROLLLEVEL = 3, PITCHLEVEL = 4, LEVELGYROROLL = 6, LEVELGYROPITCH = 7 (used for Stable Mode, accels + gyros)
// HEADING = 5 (used for heading hold)
// ALTITUDE = 8 (used for altitude hold)
// ZDAMPENING = 9 (used in altitude hold to dampen vertical accelerations)
float windupGuard; // Read in from EEPROM
// Modified from http://www.arduino.cc/playground/Main/BarebonesPIDForEspresso
float updatePID(float targetPosition, float currentPosition, struct PIDdata *PIDparameters) {
    // AKA PID experiments
    const float deltaPIDTime = (currentTime - PIDparameters->previousPIDTime) / 1000000.0;
    PIDparameters->previousPIDTime = currentTime; // AKA PID experiments
    float error = targetPosition - currentPosition;
    PIDparameters->integratedError += error * deltaPIDTime;
    PIDparameters->integratedError = constrain(PIDparameters->integratedError, -PIDparameters->windupGuard, PIDparameters->windupGuard);
    float dTerm = PIDparameters->D * (currentPosition - PIDparameters->lastPosition) / (deltaPIDTime * 100); // dT fix from Honk
    PIDparameters->lastPosition = currentPosition;
    return (PIDparameters->P * error) + (PIDparameters->I * (PIDparameters->integratedError)) + dTerm;
}
void zeroIntegralError() __attribute__((noinline));
void zeroIntegralError() {
    for (byte axis = 0; axis <= ATTITUDE_YAXIS_PID_IDX; axis++) {
        PID[axis].integratedError = 0;
        PID[axis].previousPIDTime = currentTime;
    }
}
#endif // _AQ_PID_H_
